struct Rn {
	float *f;
        uint n;
};


struct Rmn {
	struct Rn **f; 
	uint n; //only dealing with rectangles so might as well keep this
	uint m;
};

struct svd {
        struct Rn *values;
	struct Rmn *vectors;
        uint n; 
};


void destroy_Rn ( struct Rn *d) {
	if (d!=NULL) {
		delete d->f;//free(d->f);
		delete d; //free(d);
	}
	d=NULL;
}

void destroy_Rmn( struct Rmn *d) {
	if (d!=NULL) {
		for (uint i=0;i<d->m;i++)
			destroy_Rn(d->f[i]);//free(d->f[i]);
		delete d->f;//free(d->f);
		delete d;//free(d);
	}
	d=NULL;
}

void destroy_svd(struct svd *ed) {

	if (ed!=NULL) {
	destroy_Rmn(ed->vectors);
	destroy_Rn(ed->values);

	delete ed;
	}

}
struct Rn *init_Rn (uint n, bool random) {
        struct Rn *d=new Rn;//malloc(sizeof(Rn));
        d->n=n;
        d->f=new float[n]();//calloc(n,sizeof(float));
	float u1=0,u2=0;
	if (random)
		for (uint i=0;i<n;i++) {
			if (i%2==0)
				u1=u01(), u2=u01();
			d->f[i]=STDEV*n01(u1,u2,i%2);

		}
        return d;
}


struct Rn *init_eigen_Rn (uint n) {

	struct Rn *d=init_Rn(n,0);
	float u1=0,u2=0;
	for (uint i=0;i<n;i++) {
		if (i%2==0)
			u1=u01(), u2=u01();
		d->f[i]=0.025+fabs(0.10*n01(u1,u2,i%2)); ///PUTTING A LOWER BOUND OF 0.01 HERE...

	}

        return d;
}
void write_Rn(struct Rn *a, ostream &output) {

output<<a->f[0];
for (uint i=1;i<a->n;i++)
	output<<"\t"<<a->f[i];
output<<endl;

}

void write_Rn_comment(struct Rn *a, ostream &output) {

output<<"#"<<a->f[0];
for (uint i=1;i<a->n;i++)
	output<<"\t"<<a->f[i];
output<<endl;

}
void write_Rmn(struct Rmn *a, ostream &output) {

for (uint i=0;i<a->m;i++) 
	write_Rn(a->f[i],output);

}

void write_Rmn_comment(struct Rmn *a, ostream &output) {
for (uint i=0;i<a->m;i++) 
	write_Rn_comment(a->f[i],output);

}
void scale_Rn(struct Rn *d, float c) {
	for (uint i=0;i<d->n;i++)
		d->f[i]*=c;
}

void scale_Rmn(struct Rmn *d, float c) {
	for (uint i=0;i<d->m;i++)
		scale_Rn(d->f[i],c);

}

void add_Rn(struct Rn *a, struct Rn *b) {
	assert(a->n==b->n);

	for (uint i=0;i<a->n;i++)
		a->f[i]+=b->f[i];

}


float dot_product_Rn(struct Rn *a, struct Rn *b) {
	assert(a->n==b->n);
	float f=0;

	for (uint i=0;i<a->n;i++)
		f+=a->f[i]*b->f[i];

	return f;
}



void normalize_Rn(struct Rn *d) { 
	//struct Rn *return_data=init_Rn(d->n,0);

	float sum=0;
	for (uint i=0;i<d->n;i++) 
		sum+=d->f[i]*d->f[i];
	sum=sqrt(sum);

	for (uint i=0;i<d->n;i++)
		d->f[i]/=sum; //=d->f[i]/sum;	

}



struct Rmn *init_Rmn (uint m, uint n,bool random) { 
	struct Rmn *d=new Rmn;
	d->m=m;
	d->n=n;
	d->f=new Rn*[m];//calloc(m,sizeof(float*));
	for (uint i=0;i<m;i++)
		d->f[i]=init_Rn(n,random);//calloc(n,sizeof(float));
return d;
}
struct Rmn_summary {
//	float *u, **cov;
	struct Rn *x1,*x2; // first and second moments...
	uint n;
};

struct Rmn_summary *init_Rmn_summary (uint n) {
	struct Rmn_summary *d=new Rmn_summary;
	d->n=n;
	d->x1=init_Rn(n,0);
	d->x2=init_Rn(n,0);
	//d->cov=init_Rmn(n,n,0);
	return d;
}

void destroy_Rmn_summary (struct Rmn_summary *d) {
	destroy_Rn(d->x1);
	destroy_Rn(d->x2);
	delete d;	
}

struct Rn *moment_Rn(struct Rmn *d, uint n) {
	struct Rn *return_data=init_Rn(d->n,0);
	float x;

	for (uint i=0;i<d->n;i++) {
			x=0;
			for (uint k=0;k<d->m;k++) {
				x+=pow(d->f[k]->f[i],n);
			}
			x/=d->m;
			return_data->f[i]=x;
	}

	return return_data;
}


struct Rn *mean_Rn (struct Rmn *d) {
/*	struct Rn *return_data=init_Rn(d->n,0);
	float x;

	for (uint i=0;i<d->n;i++) {
			x=0;
			for (uint k=0;k<d->m;k++) {
				x+=d->f[k]->f[i];
			}
			x/=d->m;
			return_data->f[i]=x;
	}

	return return_data; */

return moment_Rn(d,1);
}

struct Rn *mean2_Rn (struct Rmn *d) { /// AKA SECOND MOMENT...
	struct Rn *return_data=init_Rn(d->n,0);
	float x;

	for (uint i=0;i<d->n;i++) {
			x=0;
			for (uint k=0;k<d->m;k++) {
				x+=d->f[k]->f[i]*d->f[k]->f[i];
			}
			x/=d->m;
			return_data->f[i]=x;
	}

	return return_data;
}
struct Rmn *cov_Rmn(struct Rmn *d) {
	struct Rmn *return_data=init_Rmn(d->n,d->n,0);

	float x,y,xy;

	for (uint i=0;i<d->n;i++) {
		for (uint j=0;j<=i;j++) { //this is symmetric
			x=0,y=0,xy=0;
			for (uint k=0;k<d->m;k++) {
				x+=d->f[k]->f[i];
				y+=d->f[k]->f[j];
				xy+=d->f[k]->f[i]*d->f[k]->f[j];
			}
			x/=d->m;
			y/=d->m;
			xy/=d->m;
			return_data->f[i]->f[j]=xy-x*y;
			return_data->f[j]->f[i]=xy-x*y; // its symmetric.
		}
	}

	return return_data;

}

struct Rmn *r2_Rmn(struct Rmn *cov) {
	struct Rmn *r2=init_Rmn(cov->n,cov->n,0);

	for (uint i=0;i<r2->n;i++)	
		for (uint j=0;j<r2->n;j++) 
			r2->f[i]->f[j]=pow(cov->f[i]->f[j]/sqrtf(cov->f[i]->f[i]*cov->f[j]->f[j]),2);
	return r2;


}

struct Rn *cp_Rn(struct Rn *src) {
	struct Rn *dst=init_Rn(src->n,0);
	for (uint i=0;i<dst->n;i++) 
		dst->f[i]=src->f[i];
	
	return dst;
}

struct Rmn *cp_Rmn(struct Rmn *src) {
	struct Rmn *dst=init_Rmn(src->m,src->n,0);
	for (uint i=0;i<dst->m;i++) {
		destroy_Rn(dst->f[i]);
		dst->f[i]=cp_Rn(src->f[i]);
	}
	return dst;
}


struct Rmn *orthogonalize_Rmn(struct Rmn *d) {
	struct Rmn *return_data=cp_Rmn(d);///=init_Rmn(d->m,d->n,0,0);
	struct Rn *mod_j;//*tmp1,*tmp2,*tmp3;

for (uint i=0;i<return_data->m;i++) { 
        for (uint j=0;j<i;j++) {
		mod_j=cp_Rn(return_data->f[j]);
//#define project_Rn(a,b)       scale_Rn(b,dot_product_Rn(a,b)/dot_product_Rn(b,b));
		scale_Rn(mod_j,-1*dot_product_Rn(return_data->f[i],mod_j)/dot_product_Rn(mod_j,mod_j));

		add_Rn(return_data->f[i],mod_j);
		delete mod_j;
        }
        normalize_Rn(return_data->f[i]);
}
	return return_data;
}



double *Rn_to_array (struct Rn *d) { // this could actually just point to the array in Rn...
	double *array=new double[d->n];

	for (uint i=0;i<d->n;i++)
		array[i]=d->f[i];

	return array;

}

double *Rmn_to_array (struct Rmn *d){
	double *array=new double[d->n*d->m];
	for (uint i=0;i<d->m;i++)
		for (uint j=0;j<d->n;j++)
			array[i*d->n+j]=d->f[i]->f[j];
	return array;
}


struct Rmn *array_to_Rmn(double *array,uint m, uint n) {
	struct Rmn *d=init_Rmn(m,n,0);

	for (uint i=0;i<m;i++)
		for (uint j=0;j<n;j++)
			d->f[i]->f[j]=array[i*d->n+j];

	return d;

}

struct Rn  *array_to_Rn(float *array, uint n) {
	struct Rn *d=init_Rn(n,0);

	for (uint i=0;i<n;i++)
		d->f[i]=array[i];

	return d;
}

struct Rmn *Rmn_averages (struct Rmn **d,uint n) {
	
	struct Rmn *return_data=init_Rmn(d[0]->m,d[0]->n,0);

	for (uint i=0;i<d[0]->m;i++)
		for (uint j=0;j<d[0]->n;j++) {
			for (uint k=0;k<n;k++)
				return_data->f[i]->f[j]+=d[k]->f[i]->f[j];
			return_data->f[i]->f[j]/=n;
		}
	return return_data;
}

struct Rmn *multiply_Rmn(struct Rmn *a, struct Rmn *b) {
	assert(a->m==b->n);
	struct Rmn *c=init_Rmn(a->m,a->m,0);

	for (uint i=0;i<a->m;i++)
		for (uint j=0;j<b->n;j++) {
			 c->f[i]->f[j]=0; // may not need this if init_Rmn is initializing properly..
			for (uint k=0;k<b->n;k++) 
				c->f[i]->f[j]+=a->f[i]->f[k]*b->f[k]->f[j];
		}
	return  c;
}
float theorize_determinant(struct Rn *eigenvalues) {
	float f=1;
	for (uint i=0;i<eigenvalues->n;i++)
		f*=eigenvalues->f[i];
	return f;
}

float calculate_determinant(struct Rmn *d) {

	uint n=d->n, i,j,i_count,j_count, count=0;
  //  double array[n-1][n-1], det=0;
	float det=0;
    if(n==1) return d->f[0]->f[0];
    if(n==2) return (d->f[0]->f[0]*d->f[1]->f[1] - d->f[0]->f[1]*d->f[1]->f[0]);

struct Rmn *array=init_Rmn(n-1,n-1,0);

    for(count=0; count<n; count++)
    {
        i_count=0;
        for(i=1; i<n; i++)
        {
            j_count=0;
            for(j=0; j<n; j++)
            {
                if(j == count) continue;
                array->f[i_count]->f[j_count] = d->f[i]->f[j];
                j_count++;
            }
            i_count++;
        }
        det += pow(-1, count) * d->f[0]->f[count] * calculate_determinant(array);
    }
    return det;

}


struct Rmn *theorize_covariance_Rmn(struct svd *ed) {
struct Rmn *vectors=ed->vectors;
struct Rn *values=ed->values;

	struct Rmn *d=init_Rmn(vectors->m,vectors->n,0);

	struct Rmn *r=NULL;
	// idea is to multiply: vectors*values*vectors^T... let's start with values*vectors^T

	for (uint i=0;i<d->m;i++)
		for (uint j=0;j<d->n;j++) {
			d->f[i]->f[j]=values->f[i]*vectors->f[j]->f[i];//vectors->f[j]->f[i]*values->f[j]; //multipling by TRANSPOSE first, so switch out 
	}
	r=multiply_Rmn(vectors,d);
	destroy_Rmn(d);

	return r;//multiply_Rmn(d,vectors);
}



struct Rmn *theorize_inverse_covariance_Rmn(struct svd *ed) {
struct Rmn *vectors=ed->vectors;
struct Rn *values=ed->values;

        struct Rmn *d=init_Rmn(vectors->m,vectors->n,0);

        struct Rmn *r=NULL;
        // idea is to multiply: vectors*values*vectors^T... let's start with values*vectors^T

        for (uint i=0;i<d->m;i++)
                for (uint j=0;j<d->n;j++) {
                        d->f[i]->f[j]=(1.f/values->f[i])*vectors->f[j]->f[i];//vectors->f[j]->f[i]*values->f[j]; //multipling by TRANSPOSE first, so switch out 
        }
        r=multiply_Rmn(vectors,d);
        destroy_Rmn(d);

        return r;//multiply_Rmn(d,vectors);
}

//this memory leaks;
/*struct Rmn *theorize_inverse_covariance_Rmn(struct svd *ed) {

	struct svd *inverse_ed=new svd;
	inverse_ed->vectors=cp_Rmn(ed->vectors);
	inverse_ed->values=cp_Rn(ed->values); //don't really need to cp it, just need the "n"...


	for (uint i=0;i<inverse_ed->values->n;i++)
		inverse_ed->values->f[i]=1.f/ed->values->f[i];

	struct Rmn *r=theorize_covariance_Rmn(inverse_ed);//multiply_Rmn(d,vectors);

	destroy_svd(inverse_ed);
	return r;
}*/
struct Rmn *theorize_Rmn(struct Rn *mu, struct svd *ed, uint m) {


	struct Rn *eigenvalues=ed->values;
	struct Rmn *eigenvectors=ed->vectors;//FIXME

	uint n=mu->n;
	


	struct Rmn *d=init_Rmn(m,n,0);

	float *iids=new float[n];
	float u1=0,u2=0;

for (uint i=0;i<m;i++) {

	for (uint j=0;j<n;j++) {
		if (j%2==0)
			u1=u01(), u2=u01();
		iids[j]=n01(u1,u2,j%2);
	}

	for (uint j=0;j<n;j++) {
		d->f[i]->f[j]=mu->f[j];
		for (uint k=0;k<n;k++)
			d->f[i]->f[j]+=sqrt(eigenvalues->f[k])*eigenvectors->f[j]->f[k]*iids[k];
	}

}

	delete iids;
	return d;
}



float log_mv_gamma(int p, float a) { // from wikipedia
float r=log(PI)*(p*(p-1))/4.f;

for (int i=1;i<=p;i++)
        r+=lgamma(a+(1-i)/2.f);

return r;

}

float mv_gamma(int p, float a) { 
return exp(log_mv_gamma(p,a));
}


float trace_Rmn (struct Rmn *d) {
	assert(d->n==d->m);
	float f=0;
	for (uint i=0;i<d->n;i++)
		f+=d->f[i]->f[i];
	return f;
}

float inverse_wishart_pdf (struct svd *ed, struct svd *true_ed) { // not quite sure what to do about the scaling matrix, leaving it as the identity for now. read "The Wishart and Inverse Wishart Distributions" by Steven Nydick.

struct Rmn *tmp_Rmn=NULL, *inverse_covariance=theorize_inverse_covariance_Rmn(ed);
uint p=ed->values->n; // dimension of our matrix...
uint m=p*(p+1)/2; // how many degrees of freedom? p(p+1)/2 in the covariance matrix. need to subtract any constaints i placed, but i can't think of any... they were generated independent from the means
//wait a minute, no... it would be the number of multivariate samples made, lets make this a parameter


struct Rmn *scale_matrix=theorize_covariance_Rmn(true_ed); //thought we were supposed to invert it, but the mode is going to be at scale_matrix/(m+p+1), which is my desired value
scale_Rmn(scale_matrix,(float)(m+p+1));  //multiply scale matrix by (m+p+1), as the mode of the inverse wishart distribution is: scale_matrix/(m+p+1)... we want the mode to be the "true" inverse covariance matrix...


float f=pow(calculate_determinant(scale_matrix),m/2.f)/(pow(theorize_determinant(ed->values),((m+p+1)/2.f))*pow(2,m*p/2.f)*mv_gamma(p,m/2.f))*expf(-0.5f*trace_Rmn(tmp_Rmn=multiply_Rmn(scale_matrix,inverse_covariance)));

destroy_Rmn(tmp_Rmn);
destroy_Rmn(scale_matrix);
destroy_Rmn(inverse_covariance);

return f;


}

float mvn_pdf(struct Rn *mu, struct svd *ed,struct Rn *true_mu) {

	struct Rn *eigenvalues=ed->values;

	//let's assume its a MNV(0,1) for now... //ANY matrix will work, but need to keep the eigenvalues as "1".
	uint n=eigenvalues->n;





		float sum=0;

	struct Rmn *inverse_covariance=theorize_inverse_covariance_Rmn(ed);


	//	cout<<"Scale matrix is:"<<endl;
//	write_Rmn(scale_matrix,cout);

	float f=powf(1/(2*PI),n*0.5f)*powf(1/theorize_determinant(eigenvalues),0.5f); //this is working properly..
	

	struct Rn *first_term=init_Rn(n,0);
	for (uint i=0;i<n;i++)
		for (uint j=0;j<n;j++)
			first_term->f[i]+=(mu->f[j]-true_mu->f[j])*inverse_covariance->f[j]->f[i];
	for (uint i=0;i<n;i++)
		sum+=first_term->f[i]*(mu->f[i]-true_mu->f[i]);

	destroy_Rn(first_term);
	destroy_Rmn(inverse_covariance);

	return f*expf(-0.5f*sum);
}


float prior_pdf(struct Rn *mu, struct svd *ed, struct Rn *true_mu, struct svd *true_ed) {


	//this is under the assumption that the prior is: N(0,I)... i think we only change the inverse wishart scale matrix to be the sum of squares of our assumed variance, not quite sure yet.
	return mvn_pdf(mu,ed,true_mu)*inverse_wishart_pdf(ed,true_ed);

}

struct Rmn *perturb_covariance_Rmn (struct svd *ed, uint m ) { //m is the number of samples to use... the larger the number, the tighter the covariance...


struct Rn *mu=init_Rn(ed->values->n,0);
struct Rmn *data=theorize_Rmn(mu,ed,m);

	struct Rmn *cov=cov_Rmn(data);
	destroy_Rmn(data);
	destroy_Rn(mu);

	return cov; //placehold

}



struct Rmn *cholesky_Rmn(struct Rmn *A) {
	uint n=A->n;

    //double *L = (double*)calloc(n * n, sizeof(double));
	struct Rmn *L=init_Rmn(n,n,0);

    for (uint i = 0; i < n; i++)
        for (uint j = 0; j < (i+1); j++) {
            double s = 0;
            for (uint k = 0; k < j; k++)
                s += L->f[i]->f[k] * L->f[j]->f[k];
            L->f[i]->f[j] = (i == j) ?
                           sqrt(A->f[i]->f[i] - s) :
                           (1.0 / L->f[j]->f[j] * (A->f[i]->f[j] - s));
        }

    return L;
}

/* svd.c: Perform a singular value decomposition A = USV' of square matrix.
 *
 * This routine has been adapted with permission from a Pascal implementation
 * (c) 1988 J. C. Nash, "Compact numerical methods for computers", Hilger 1990.
 * The A matrix must be pre-allocated with 2n rows and n columns. On calling
 * the matrix to be decomposed is contained in the first n rows of A. On return
 * the n first rows of A contain the product US and the lower n rows contain V
 * (not V'). The S2 vector returns the square of the singular values.
 *
 * (c) Copyright 1996 by Carl Edward Rasmussen. */


struct svd *svd_svd(struct Rmn *d) { //maybe there's no need to have an initialize function...
uint n=d->n;
struct svd *ret=new svd;



if (n==1) {
	ret->vectors=init_Rmn(1,1,0);
	ret->values=init_Rn(1,0);
	ret->vectors->f[0]->f[0]=1;
	ret->values->f[0]=d->f[0]->f[0];
	return ret;
}




struct Rmn *A=init_Rmn(2*n,n,0);
struct Rn *S2=init_Rn(n,0);

for (uint i=0;i<n;i++)
	for (uint j=0;j<n;j++)
		A->f[i]->f[j]=d->f[i]->f[j];

  uint  i, j, k, EstColRank = n, RotCount = n, SweepCount = 0,
       slimit = (n<120) ? 30 : n/4;
  double eps = 1e-15, e2 = 10.0*n*eps*eps, tol = 0.1*eps, vt, p, x0,
       y0, q, r, c0, s0, d1, d2;

  for (i=0; i<n; i++) { for (j=0; j<n; j++) A->f[n+i]->f[j] = 0.0; A->f[n+i]->f[i] = 1.0; }
  while (RotCount != 0 && SweepCount++ <= slimit) {
    RotCount = EstColRank*(EstColRank-1)/2;
    for (j=0; j<EstColRank-1; j++) 
      for (k=j+1; k<EstColRank; k++) {
        p = q = r = 0.0;
        for (i=0; i<n; i++) {
          x0 = A->f[i]->f[j]; y0 = A->f[i]->f[k];
          p += x0*y0; q += x0*x0; r += y0*y0;
        }
        S2->f[j] = q; S2->f[k] = r;
        if (q >= r) {
          if (q<=e2*S2->f[0] || fabs(p)<=tol*q)
            RotCount--;
          else {
            p /= q; r = 1.0-r/q; vt = sqrt(4.0*p*p+r*r);
            c0 = sqrt(0.5*(1.0+r/vt)); s0 = p/(vt*c0);
            for (i=0; i<2*n; i++) {
              d1 = A->f[i]->f[j]; d2 = A->f[i]->f[k];
              A->f[i]->f[j] = d1*c0+d2*s0; A->f[i]->f[k] = -d1*s0+d2*c0;
            }
          }
        } else {
          p /= r; q = q/r-1.0; vt = sqrt(4.0*p*p+q*q);
          s0 = sqrt(0.5*(1.0-q/vt));
          if (p<0.0) s0 = -s0;
          c0 = p/(vt*s0);
          for (i=0; i<2*n; i++) {
            d1 = A->f[i]->f[j]; d2 = A->f[i]->f[k];
            A->f[i]->f[j] = d1*c0+d2*s0; A->f[i]->f[k] = -d1*s0+d2*c0;
          }
        }
      }
    while (EstColRank>2 && S2->f[EstColRank-1]<=S2->f[0]*tol+tol*tol) EstColRank--;
  }
//  if (SweepCount > slimit)
  //  printf("Warning: Reached maximum number of sweeps (%d) in SVD routine...\n"
//	   ,slimit);

struct Rmn *eigenvectors=init_Rmn(n,n,0);

for (i=0;i<n;i++)  {
	S2->f[i]=sqrt(S2->f[i]);
	for (j=0;j<n;j++)
		eigenvectors->f[i]->f[j]=A->f[n+i]->f[j];

}
destroy_Rmn(A);



ret->vectors=eigenvectors;
ret->values=S2;

return ret;

}

struct Rmn *permute_Rmn(struct Rmn *d) {

struct Rn *tmp_Rn=NULL;
uint j=0;

for (uint i=0;i<d->m;i++) {
	tmp_Rn=d->f[i];
	j=(uint)(((float)d->m)*u01());
	d->f[i]=d->f[j];
	d->f[j]=tmp_Rn;
}	


return d;


}
